using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using System.Diagnostics;

namespace Imagination.Physics
{
    public class RigidBodyPhysicsService : PhysicsService
    {
        private ImaginationEngine engine;

        public RigidBodyPhysicsService(Game game) : base(game)
        {
            engine = game.Services.GetService(typeof(ImaginationEngine)) as ImaginationEngine;
            
            // HACK: this shouldn't be hard-coded into the physics service
            Vector3 gravity = new Vector3(0, -9.8f, 0);
            Vector3 reverseGravity = new Vector3(0, 9.8f, 0);

            SetConstant<Vector3>("gravity", gravity);
            SetConstant<Force>("gravity", new Force(gravity));
            SetConstant<Vector3>("reverseGravity", reverseGravity);
            SetConstant<Force>("reverseGravity", new Force(reverseGravity));
        }

        public override void Calculate(GameTime gameTime, Thing thing)
        {
            float dt = (float)(gameTime.ElapsedGameTime.TotalSeconds);
            if (!thing.IsLocked)
            {
                Matrix originalWorld = thing.World;

                foreach (Constraint constraint in thing.Constraints)
                    constraint.Update(gameTime, thing);

                //ApplyConstraints(dt);

                //	friction
                //moments += Vector3.(this.Force);

                // todo: fix this code!
                //thing.AngularAcceleration = Vector3.TransformNormal(
                //    thing.Moments - Vector3.Cross(thing.AngularVelocity,
                //    Vector3.TransformNormal(thing.AngularVelocity, 
                //    thing.Inertia)), thing.InverseInertia);

                Vector3 force = thing.Force;
                thing.Acceleration = force * thing.InverseMass;
                thing.ClearTemporaryForces();

                thing.Velocity += thing.Acceleration * dt;
                thing.Position += thing.Velocity * dt;
                
                thing.AngularVelocity += thing.AngularAcceleration * dt;

                thing.Orientation *= Quaternion.CreateFromAxisAngle(
                    thing.AngularVelocity, thing.AngularVelocity.Length() * dt);
                thing.Orientation.Normalize();

                thing.World = Matrix.CreateScale(thing.Scale) *
                    Matrix.CreateFromQuaternion(thing.Orientation) *
                    Matrix.CreateTranslation(thing.Position);

                if (!originalWorld.Equals(thing.World))
                {
                    counterThingsMoved++;
                    thing.Boundary.Center = thing.Position;
                    thing.Zone.ThingMoved(thing);
                }
            }
        }

        public override void InitializePhysics(Thing thing)
        {
            Force gravity = GetConstant<Force>("gravity");
            thing.Forces.Add(gravity);
        }

        public override string ToString()
        {
            return String.Format("RigidBodyPhysicsService: {0} Things moved", counterThingsMoved);
        }
    }
}
